#pragma once

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/search/kdtree.h>
#include <pcl/common/common.h>

#include <opencv2/opencv.hpp>

#include <iostream>
#include <string>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <unordered_map>

using PointT = pcl::PointXYZ;
using PointCloudT = pcl::PointCloud<PointT>;

class PcdToDepthProcessor {
public:
    PcdToDepthProcessor();
    ~PcdToDepthProcessor();
    
    // 主处理函数
    bool processPointCloud(const std::string& pcd_file);
    
private:
    // 点云相关
    PointCloudT::Ptr original_cloud_;
    PointCloudT::Ptr filtered_cloud_;
    
    // 深度图相关
    cv::Mat depth_image_;
    
    // 添加保存滤波索引的容器
    std::vector<bool> filtered_indices_;
    
    // 参数
    struct Parameters {
        // Z轴直通滤波参数
        float z_min = -115.0f;     // Z轴最小值
        float z_max = -60.0f;      // Z轴最大值
        
        // 半径滤波参数
        double radius_search = 15;     // 搜索半径 (米) - 增大搜索半径
        int min_neighbors = 80;           // 最少邻居点数量 - 减少邻居数要求
        
        // 体素滤波参数
        float voxel_leaf_size = 1.0f;  // 体素网格叶子大小 (米)
        
        // 边缘检测参数
        double canny_threshold1 = 50.0;   // Canny边缘检测低阈值
        double canny_threshold2 = 100.0;  // Canny边缘检测高阈值
        int gaussian_kernel_size = 5;     // 高斯模糊核大小
        double gaussian_sigma = 1.0;      // 高斯模糊标准差
        
        // 深度图预处理参数
        int morphology_kernel_size = 7;   // 形态学操作核大小
        int median_kernel_size = 5;       // 中值滤波核大小
        int bilateral_d = 5;              // 双边滤波邻域直径
        double bilateral_sigma_color = 100.0;   // 双边滤波颜色标准差
        double bilateral_sigma_space = 200.0;   // 双边滤波空间标准差
        
        // 固定深度图尺寸 (对应原始点云尺寸)
        int image_width = 3200;    // 固定宽度
        int image_height = 1400;   // 固定高度
        
        // 路径点数控制参数
        int path_point_count = 45;
        
        // 路径内偏移参数
        float path_inward_offset = 2.0f;  // 路径向内偏移距离（像素），用于打胶路径调参
        
        // 点云边界（用于映射计算）
        float x_min = 0, x_max = 0;
        float y_min = 0, y_max = 0;
    } params_;
    
    // 内部处理函数
    bool loadPointCloud(const std::string& pcd_file);
    void applyZFilter();
    void applyVoxelFilter();
    void applyRadiusFilter();
    void calculatePointCloudBounds();
    void convertToDepthImage();
    void preprocessDepthImage();
    void extractEdgesAndPath();
    void savePathCoordinates(const std::vector<std::vector<cv::Point>>& contours);
    void saveResults();
    
    // 辅助函数
    std::vector<cv::Point> resampleContour(const std::vector<cv::Point>& contour, int target_points);
    std::vector<cv::Point2f> applyInwardOffset(const std::vector<cv::Point>& contour, float offset_pixels);
    float getDepthAtPixel(int x, int y) const;
    cv::Point2f calculateDepthImageCentroid() const;
};

// 辅助函数
bool fileExists(const std::string& filename);
